#!/usr/bin/env python3

import hybrid_astar as ha
import MPC_XY_Frame as mxf
import math
import rospy
from geometry_msgs.msg import Twist
import time
import sys
import matplotlib.pyplot as plt
import copy
from mvccep_image_localization.msg import base_localization
robot_num = "robot_2"


def callback(data):
    x = data.robot_2.pos.x
    y = data.robot_2.pos.y
    yaw = data.robot_2.angle
    print("x:",x,"y:",y,"yaw:",yaw)
    
    twist.linear.x = 0.1               # 线速度 m/s
    twist.angular.z = 0.1             # 倾角 rad
    cmd_pub.publish(twist)


if __name__ == '__main__':
    yaw = math.atan2(-1-0,-1-0)
    print(10%2)
    # 节点初始化
    # global cmd_pub
    # rospy.init_node("one_robot_astar_mpc",anonymous=False)  # 节点
    # cmd_pub = rospy.Publisher("/" + robot_num + "/cmd_vel" ,Twist, queue_size=1)  # 发布者
    # rospy.Subscriber("base_localization" ,base_localization, callback,queue_size=1) # 接受者
    # twist = Twist()     # 发布对象:速度类型
    # rospy.spin()